#ifndef BINARYSEARCHTREECORE_H_INCLUDED
#define BINARYSEARCHTREECORE_H_INCLUDED

#include "Assertions.h"
#include "Constants.h"

namespace CEngine
{

template<class E, class K>
BinarySearchTree<E,K>::BinarySearchTree(void)
    :   array(),
        root(NULL),
        keyOffset(0),
        minKeyPointer(NULL),
        currentKey(NULL),
        currentParentStack(),
        maxKeyPointer(NULL),
        maxKey(K()),
        minKey(K()),
        relativeKey(false)
{}

template<class E, class K>
BinarySearchTree<E,K>::BinarySearchTree(const E& element, const K& key)
    :   array(),
        root(NULL),
        keyOffset(0),
        minKeyPointer(NULL),
        currentKey(NULL),
        currentParentStack(),
        maxKeyPointer(NULL),
        maxKey(K()),
        minKey(K()),
        relativeKey(true)
{
    keyOffset = (U64)((U8*)&key - (U8*)&element);

    Node node;
    node.element = element;
    node.key = key;
    node.arrayPosition = 0;

    minKey = key;
    maxKey = key;

    array.pushBack(node);
    root = &array.front();
    currentKey = root;
    minKeyPointer = root;
    maxKeyPointer = root;
}

template<class E, class K>
BinarySearchTree<E,K>::BinarySearchTree(const BinarySearchTree<E,K>& tree)
    :   array(tree.array),
        root(array.front()),
        keyOffset(tree.keyOffset),
        minKeyPointer(NULL),
        currentKey(NULL),
        currentParentStack(),
        maxKeyPointer(NULL),
        maxKey(tree.maxKey),
        minKey(tree.minKey),
        relativeKey(tree.relativeKey)

{
    minKeyPointer = (tree.minKeyPointer - tree.array.getRawArray()) + array.getRawArray();
    currentKey = findElement(tree.currentKey->key, root);
    maxKeyPointer = (tree.maxKeyPointer - tree.array.getRawArray()) + array.getRawArray();
}

template<class E, class K>
BinarySearchTree<E,K>::BinarySearchTree(Vector<E>& vector, const K& key)
    :   array(),
        root(NULL),
        keyOffset(0),
        minKeyPointer(NULL),
        currentKey(NULL),
        currentParentStack(),
        maxKeyPointer(NULL),
        maxKey(K()),
        minKey(K()),
        relativeKey(true)
{
    ///key must be from the first element of the vector!
    ASSERT(&key < (K*)(vector.getRawArray() + 2));

    array.reserve(vector.getSize());

    keyOffset = (U64)((U8*)&key - (U8*)vector.getRawArray());

    Node node;
    node.element = vector.front();
    node.key = key;
    node.arrayPosition = 0;
    minKey = key;
    maxKey = key;

    array.pushBack(node);
    root = &array[0];
    minKeyPointer = root;
    maxKeyPointer = root;
    currentKey = root;

    for (U64 i = 1; i < vector.getSize(); ++i) {
        insert(++vector);
    }
}

template<class E, class K>
BinarySearchTree<E,K>&
BinarySearchTree<E,K>::operator=(const BinarySearchTree& tree) {
    if(this == &tree) {
        return *this;
    }

    array.clear();
    currentParentStack.clear();
    root = NULL;
    minKeyPointer = NULL;
    maxKeyPointer = NULL;
    currentKey = NULL;

    array = tree.array;
    currentParentStack = tree.currentParentStack;
    keyOffset = tree.keyOffset;
    relativeKey = tree.relativeKey;
    if(tree.root) {
        root = (tree.root - tree.array.getRawArray());
    }
    if(tree.minKeyPointer) {
        minKeyPointer = (tree.minKeyPointer - tree.array.getRawArray());
    }
    if(tree.maxKeyPointer) {
        maxKeyPointer = (tree.maxKeyPointer - tree.array.getRawArray());
    }
    if(currentKey) {
        currentKey = (tree.currentKey - tree.array.getRawArray());
    }
    minKey = tree.minKey;
    maxKey = tree.maxKey;

    return *this;
}

template<class E, class K>
BinarySearchTree<E,K>&
BinarySearchTree<E,K>::operator=(Vector<E>& vector) {
    array.clear();
    currentParentStack.clear();
    root = NULL;
    minKeyPointer = NULL;
    maxKeyPointer = NULL;
    currentKey = NULL;

    Node node;
    node.element = vector.front();
    node.key = *(K*)((void*)&node.element + keyOffset);
    node.arrayPosition = 0;
    minKey = node.key;
    maxKey = node.key;

    array.pushBack(node);
    root = &array[0];
    minKeyPointer = root;
    maxKeyPointer = root;
    currentKey = root;

    for (U64 i = 1; i < vector.getSize(); ++i) {
        insert(++vector);
    }

    return *this;
}

template<class E, class K>
BinarySearchTree<E,K>::~BinarySearchTree(void) {
    root = NULL;
    keyOffset = 0;

    minKeyPointer = NULL;
    currentKey = NULL;
    maxKeyPointer = NULL;

    relativeKey = false;

}

template<class E, class K>
E& BinarySearchTree<E,K>::min(void) const {
    if(getSize() == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }

    return minKeyPointer->element;
}

template<class E, class K>
E& BinarySearchTree<E,K>::current(void) const {
    if(getSize() == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }

    return currentKey->element;
}

template<class E, class K>
E& BinarySearchTree<E,K>::max(void) const {
    if(getSize() == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }

    return maxKeyPointer->element;
}

template<class E, class K>
E& BinarySearchTree<E,K>::operator[](const K& key) {
    if(getSize() == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }

    currentParentStack.clear();

    if(key < minKey || key > maxKey) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_WARNING, "Attemped to access element outside container's range!\n");
        currentKey = root;
        return currentKey->element;
    }

    currentKey = findElement(key, root);

    return currentKey->element;
}

template<class E, class K>
E& BinarySearchTree<E,K>::operator++(void) {
    if(getSize() == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }
    else if(currentKey == maxKeyPointer) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_WARNING, "Attempted to increment iterator beyond container range!\n");
        return currentKey->element;
    }

    Node* previousKey = NULL;
    while(currentKey != maxKeyPointer) {
        if(previousKey != NULL && currentKey->key > previousKey->key) {
            break;
        }
        if(currentKey->right != NULL && currentKey->right != previousKey) {
            currentParentStack.pushBack(currentKey);
            currentKey = currentKey->right;
            while(currentKey->left != NULL) {
                currentParentStack.pushBack(currentKey);
                currentKey = currentKey->left;
            }
            break;
        }

        previousKey = currentKey;
        currentKey = currentParentStack.back();
        currentParentStack.popBack();
    }

    return currentKey->element;
}

template<class E, class K>
E& operator++(BinarySearchTree<E,K>& tree, int) {
    E* temp = &tree.current();
    ++tree;
    return *temp;
}

template<class E, class K>
E& BinarySearchTree<E,K>::operator--(void) {
    if(getSize() == 0) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_ERROR, "Accessed element in empty container!\n");
    }
    else if(currentKey == minKeyPointer) {
        defaultLog.log(CEngine::Logging::CONTAINERS, LOG_WARNING, "Attempted to increment iterator beyond container range!\n");
        return currentKey->element;
    }

    Node* previousKey = NULL;
    while(currentKey != minKeyPointer) {
        if(previousKey != NULL &&currentKey->key < previousKey->key) {
            break;
        }
        if(currentKey->left != NULL && currentKey->left != previousKey) {
            currentParentStack.pushBack(currentKey);
            currentKey = currentKey->left;
            while(currentKey->right != NULL) {
                currentParentStack.pushBack(currentKey);
                currentKey = currentKey->right;
            }
            break;
        }

        previousKey = currentKey;
        currentKey = currentParentStack.back();
        currentParentStack.popBack();
    }

    return currentKey->element;
}

template<class E, class K>
void BinarySearchTree<E,K>::insert(const E& element) {
    if(!relativeKey) {
        defaultLog.log(Logging::CONTAINERS, LOG_ERROR, "relativeKey must be set to use relative key insert!\n");
        return;
    }

    K key = *(K*)((void*)&element + keyOffset);

    if(root == NULL) {
        Node node;
        node.element = element;
        node.key = key;
        node.arrayPosition = 0;
        array.pushBack(node);
        root = &array.back();

        minKey = key;
        maxKey = key;

        minKeyPointer = root;
        currentKey = root;
        maxKeyPointer = root;
    }
    else if(key < minKey) {
        insertElement(key, element, *minKeyPointer);
        minKey = key;
        minKeyPointer = &array.back();
    }
    else if(key > maxKey) {
        insertElement(key, element, *maxKeyPointer);
        maxKey = key;
        maxKeyPointer = &array.back();
    }
    else {
        insertElement(key , element, *root);
    }
}

template<class E, class K>
void BinarySearchTree<E,K>::insert(const E& element, const K& key) {
    if(relativeKey) {
        defaultLog.log(Logging::CONTAINERS, LOG_WARNING, "Relative key insert should be used when relativeKey is set!\n");
    }

    if(root == NULL) {
        Node node;
        node.element = element;
        node.key = key;
        node.arrayPosition = 0;
        array.pushBack(node);
        root = &array.back();

        minKey = key;
        maxKey = key;

        minKeyPointer = root;
        currentKey = root;
        maxKeyPointer = root;
    }
    else if(key < minKey) {
        insertElement(key, element, *minKeyPointer);
        minKey = key;
        minKeyPointer = &array.back();
    }
    else if(key > maxKey) {
        insertElement(key, element, *maxKeyPointer);
        maxKey = key;
        maxKeyPointer = &array.back();
    }
    else {
        insertElement(key, element, *root);
    }
}

template<class E, class K>
void BinarySearchTree<E,K>::erase(void) {
    if(getSize() == 0) {
        return;
    }

    U8 specialNode = 0;
    if(currentKey == root) {
        specialNode += 1;
    }
    if(currentKey == minKeyPointer) {
        specialNode += 2;
    }
    if(currentKey == maxKeyPointer) {
        specialNode += 4;
    }
    U64 arrayPosition;
    if(currentKey->right != NULL) {
        if((specialNode & 1) == 1) {
            Node* temp = root;
            ++(*this);

            root = currentKey;
            root->right = temp->right;
            if(temp->left == NULL) {
                minKeyPointer = currentKey;
                minKey = minKeyPointer->key;
            }
            else {
                root->left = temp->left;
            }

            if(currentParentStack.getSize() > 1) {
                currentParentStack.back()->left = NULL;
            }
            currentParentStack.clear();

            arrayPosition = temp->arrayPosition;
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
        else {
            Node* temp = currentKey;
            Node* parent = currentParentStack.back();
            ++(*this);

            if((specialNode & 2) == 2) {
                minKeyPointer = currentKey;
                minKey = minKeyPointer->key;
            }

            if(currentKey->key < currentParentStack.back()->key) {
                currentParentStack.back()->left = currentKey->right;
            }
            else {
                currentParentStack.back()->right = currentKey->right;
            }
            if(currentKey->key < parent->key) {
                parent->left = currentKey;
            }
            else {
                parent->right = currentKey;
            }
            currentKey->left = temp->left;
            currentKey->right = temp->right;

            arrayPosition = temp->arrayPosition;
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition ; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
    }
    else if(currentKey->left != NULL) {
        if((specialNode & 1) == 1) {
            Node* temp = root;
            --(*this);

            root = currentKey;
            root->left = temp->left;
            if(temp->right == NULL) {
                maxKeyPointer = currentKey;
                maxKey = maxKeyPointer->key;
            }
            else {
                root->right = temp->right;
            }

            if(currentParentStack.getSize() > 1) {
                currentParentStack.back()->right = NULL;
            }
            currentParentStack.clear();

            arrayPosition = temp->arrayPosition;
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
        else {
            Node* temp = currentKey;
            Node* parent = currentParentStack.back();
            --(*this);

            if((specialNode & 4) == 4) {
                maxKeyPointer = currentKey;
                maxKey = maxKeyPointer->key;
            }

            if(currentKey->key < currentParentStack.back()->key) {
                currentParentStack.back()->left = currentKey->left;
            }
            else {
                currentParentStack.back()->right = currentKey->left;
            }
            if(currentKey->key < parent->key) {
                parent->left = currentKey;
            }
            else {
                parent->right = currentKey;
            }
            currentKey->left = temp->left;
            currentKey->right = temp->right;

            arrayPosition = temp->arrayPosition;
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
    }
    else {
        if((specialNode & 1) == 1) {
            array.clear();
            root = NULL;

            currentKey = NULL;
            currentParentStack.clear();

            minKeyPointer = NULL;
            maxKeyPointer = NULL;
            minKey = 0;
            maxKey = 0;
        }
        else {
            if(currentKey->key < currentParentStack.back()->key) {
                currentParentStack.back()->left = NULL;
            }
            else {
                currentParentStack.back()->right = NULL;
            }

            Node* temp = currentKey;
            if((specialNode & 2) == 2) {
                ++(*this);
                minKeyPointer = currentKey;
                minKey = minKeyPointer->key;
            }
            else if((specialNode & 4) == 4) {
                --(*this);
                maxKeyPointer = currentKey;
                maxKey = maxKeyPointer->key;
            }

            arrayPosition = temp->arrayPosition;
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
    }
}

template<class E, class K>
void BinarySearchTree<E,K>::erase(const K& key) {
    if(getSize() == 0) {
        return;
    }

    K keyTemp = currentKey->key;
    (*this)[key];

    U8 specialNode = 0;
    if(currentKey == root) {
        specialNode += 1;
    }
    if(currentKey == minKeyPointer) {
        specialNode += 2;
    }
    if(currentKey == maxKeyPointer) {
        specialNode += 4;
    }
    U64 arrayPosition;
    if(currentKey->right != NULL) {
        if((specialNode & 1) == 1) {
            Node* temp = root;
            ++(*this);

            root = currentKey;
            root->right = temp->right;
            if(temp->left == NULL) {
                minKeyPointer = currentKey;
                minKey = minKeyPointer->key;
            }
            else {
                root->left = temp->left;
            }

            if(currentParentStack.getSize() > 1) {
                currentParentStack.back()->left = NULL;
            }
            currentParentStack.clear();

            arrayPosition = temp->arrayPosition;
            if(root->arrayPosition > arrayPosition) {
                --root;
            }
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
        else {
            Node* temp = currentKey;
            Node* parent = currentParentStack.back();
            ++(*this);

            if((specialNode & 2) == 2) {
                minKeyPointer = currentKey;
                minKey = minKeyPointer->key;
            }

            if(currentKey->key < currentParentStack.back()->key) {
                currentParentStack.back()->left = currentKey->right;
            }
            else {
                currentParentStack.back()->right = currentKey->right;
            }
            if(currentKey->key < parent->key) {
                parent->left = currentKey;
            }
            else {
                parent->right = currentKey;
            }
            currentKey->left = temp->left;
            currentKey->right = temp->right;

            arrayPosition = temp->arrayPosition;
            if(root->arrayPosition > arrayPosition) {
                --root;
            }
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition ; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
    }
    else if(currentKey->left != NULL) {
        if((specialNode & 1) == 1) {
            Node* temp = root;
            --(*this);

            root = currentKey;
            root->left = temp->left;
            if(temp->right == NULL) {
                maxKeyPointer = currentKey;
                maxKey = maxKeyPointer->key;
            }
            else {
                root->right = temp->right;
            }

            if(currentParentStack.getSize() > 1) {
                currentParentStack.back()->right = NULL;
            }
            currentParentStack.clear();

            arrayPosition = temp->arrayPosition;
            if(root->arrayPosition > arrayPosition) {
                --root;
            }
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
        else {
            Node* temp = currentKey;
            Node* parent = currentParentStack.back();
            --(*this);

            if((specialNode & 4) == 4) {
                maxKeyPointer = currentKey;
                maxKey = maxKeyPointer->key;
            }

            if(currentKey->key < currentParentStack.back()->key) {
                currentParentStack.back()->left = currentKey->left;
            }
            else {
                currentParentStack.back()->right = currentKey->left;
            }
            if(currentKey->key < parent->key) {
                parent->left = currentKey;
            }
            else {
                parent->right = currentKey;
            }
            currentKey->left = temp->left;
            currentKey->right = temp->right;

            arrayPosition = temp->arrayPosition;
            if(root->arrayPosition > arrayPosition) {
                --root;
            }
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
    }
    else {
        if((specialNode & 1) == 1) {
            array.clear();
            root = NULL;

            currentKey = NULL;
            currentParentStack.clear();

            minKeyPointer = NULL;
            maxKeyPointer = NULL;
            minKey = 0;
            maxKey = 0;
        }
        else {
            if(currentKey->key < currentParentStack.back()->key) {
                currentParentStack.back()->left = NULL;
            }
            else {
                currentParentStack.back()->right = NULL;
            }

            Node* temp = currentKey;
            if((specialNode & 2) == 2) {
                ++(*this);
                minKeyPointer = currentKey;
                minKey = minKeyPointer->key;
            }
            else if((specialNode & 4) == 4) {
                --(*this);
                maxKeyPointer = currentKey;
                maxKey = maxKeyPointer->key;
            }

            arrayPosition = temp->arrayPosition;
            if(root->arrayPosition > arrayPosition) {
                --root;
            }
            if(minKeyPointer->arrayPosition > arrayPosition) {
                --minKeyPointer;
            }
            if(maxKeyPointer->arrayPosition > arrayPosition) {
                --maxKeyPointer;
            }
            if(currentKey->arrayPosition > arrayPosition) {
                --currentKey;
            }
            array.erase(arrayPosition);
            for (U64 i = arrayPosition + 1; i < array.getSize(); ++i) {
                --array[i].arrayPosition;
            }
            for (U64 i = 0; i < array.getSize(); ++i) {
                if(array[i].left > (array.getRawArray() + arrayPosition)) {
                    --array[i].left;
                }
                if(array[i].right > (array.getRawArray() + arrayPosition)) {
                    --array[i].right;
                }
            }
        }
    }

    (*this)[keyTemp];
}

template<class E, class K>
bool BinarySearchTree<E,K>::isEmpty(void) const {
    return !root;
}

template<class E, class K>
U64 BinarySearchTree<E,K>::getSize(void) const {
    return array.getSize();
}

template<class E, class K>
auto BinarySearchTree<E,K>::getRawVector(void) const -> Vector<Node>& {
    return array;
}

template<class E, class K>
void BinarySearchTree<E,K>::insertElement(const K& key, const E& element, Node& node) {
    if(key < node.key) {
        if(node.left != NULL) {
            insertElement(key, element, *node.left);
        }
        else {
            Node newNode;
            newNode.element = element;
            newNode.key = key;

            array.pushBack(newNode);
            array.back().arrayPosition = array.getSize() - 1;
            node.left = &array.back();
        }
    }
    else {
        if(node.right != NULL) {
            insertElement(key, element, *node.right);
        }
        else {
            Node newNode;
            newNode.element = element;
            newNode.key = key;

            array.pushBack(newNode);
            array.back().arrayPosition = array.getSize() - 1;
            node.right = &array.back();
        }
    }
}

template<class E, class K>
auto BinarySearchTree<E,K>::findElement(const K& key, Node* node, bool addToStack) -> Node* {
    if(node) {
        if(addToStack) {
            currentParentStack.pushBack(node);
        }

        if(key == node->key) {
            if(addToStack) {
                currentParentStack.popBack();
            }
            return node;
        }
        else if(key < node->key) {
            return findElement(key, node->left, addToStack);
        }
        else {
            return findElement(key, node->right, addToStack);
        }
    }
    else {
        return NULL;
    }
}

}

#endif // BINARYSEARCHTREECORE_H_INCLUDED
